/**************************************************************************************************
* Title: SumQuaternion.inl
* Author: Gael Huber
* Description: Implementations for quaternion functionality
**************************************************************************************************/

/**************************************************************************************************
* Default constructor
**************************************************************************************************/
inline Quaternion::Quaternion(void) 
	: x(0.0f), y(0.0f), z(0.0f), w(0.0f)
{ }

/**************************************************************************************************
* Quaternion from array
**************************************************************************************************/
inline Quaternion::Quaternion(const float* f)
	: x(f[0]), y(f[1]), z(f[2]), w(f[3])
{ }

/**************************************************************************************************
* Quaternion from given values
**************************************************************************************************/
inline Quaternion::Quaternion(float x, float y, float z, float w)
	: x(x), y(y), z(z), w(w)
{ }

/**************************************************************************************************
* Copy constructor
**************************************************************************************************/
inline Quaternion::Quaternion(const Quaternion& rhs)
	: x(rhs.x), y(rhs.y), z(rhs.z), w(rhs.w)
{ }

/**************************************************************************************************
* Destructor
**************************************************************************************************/
inline Quaternion::~Quaternion(void)
{ }

/**************************************************************************************************
* Returns data as float*
**************************************************************************************************/
inline Quaternion::operator float*(void) {
	return (float*) this;
}

/**************************************************************************************************
* Returns data as const float*
**************************************************************************************************/
inline Quaternion::operator const float*(void) const {
	return (const float*) this;
}

/**************************************************************************************************
* Assignment operator
**************************************************************************************************/
inline Quaternion& Quaternion::operator=(const Quaternion& rhs) {
	// Self-assignment guard
	if(this == &rhs)
		return *this;

	// Assign and return
	x = rhs.x;
	y = rhs.y;
	z = rhs.z;
	w = rhs.w;
	return *this;
}

/**************************************************************************************************
* Adds a quaternion and return the modified object
**************************************************************************************************/
inline Quaternion& Quaternion::operator+=(const Quaternion& rhs) {
	x += rhs.x;
	y += rhs.y;
	z += rhs.z;
	w += rhs.w;
	return *this;
}

/**************************************************************************************************
* Subtract a quaternion and return the modified object
**************************************************************************************************/
inline Quaternion& Quaternion::operator-=(const Quaternion& rhs) {
	x -= rhs.x;
	y -= rhs.y;
	z -= rhs.z;
	w -= rhs.w;
	return *this;
}

/**************************************************************************************************
* Multiply by another quaternion and return the result
**************************************************************************************************/
inline Quaternion& Quaternion::operator*=(const Quaternion& rhs) {
	float X = w*rhs.x + x*rhs.w + z*rhs.y - y*rhs.z;
	float Y = w*rhs.y + y*rhs.w + x*rhs.z - z*rhs.x;
	float Z = w*rhs.z + z*rhs.w + y*rhs.x - x*rhs.y;
	w = w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z;
	x = X;
	y = Y;
	z = Z;
	return *this;
}

/**************************************************************************************************
* Multiply by a scalar and return the result
**************************************************************************************************/
inline Quaternion& Quaternion::operator*=(float rhs) {
	x *= rhs;
	y *= rhs;
	z *= rhs;
	w *= rhs;
	return *this;
}

/**************************************************************************************************
* Divide by a scalar and return the result
**************************************************************************************************/
inline Quaternion& Quaternion::operator/=(float rhs) {
	const float f = 1.0f / rhs;
	x *= f;
	y *= f;
	z *= f;
	w *= f;
	return *this;
}

/**************************************************************************************************
* Return a positive version of the quaternion
**************************************************************************************************/
inline Quaternion Quaternion::operator+(void) const {
	return Quaternion(x, y, z, w);
}

/**************************************************************************************************
* Return a negated version of the quaternion
**************************************************************************************************/
inline Quaternion Quaternion::operator-(void) const {
	return Quaternion(-x, -y, -z, -w);
}

/**************************************************************************************************
* Returns the sum of two vectors
**************************************************************************************************/
inline Quaternion Quaternion::operator+(const Quaternion& rhs) const {
	return Quaternion(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
}

/**************************************************************************************************
* Returns the difference between two vectors
**************************************************************************************************/
inline Quaternion Quaternion::operator-(const Quaternion& rhs) const {
	return Quaternion(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
}

/**************************************************************************************************
* Returns product of two quaternions
**************************************************************************************************/
inline Quaternion Quaternion::operator*(const Quaternion& rhs) const {
	return Quaternion(
		w*rhs.x + x*rhs.w + z*rhs.y - y*rhs.z,
		w*rhs.y + y*rhs.w + x*rhs.z - z*rhs.x,
		w*rhs.z + z*rhs.w + y*rhs.x - x*rhs.y,
		w*rhs.w - x*rhs.x - y*rhs.y - z*rhs.z);
}

/**************************************************************************************************
* Returns the product of a quaternion and a scalar
**************************************************************************************************/
inline Quaternion Quaternion::operator*(float rhs) const {
	return Quaternion(
		x*rhs,
		y*rhs,
		z*rhs,
		w*rhs);
}

/**************************************************************************************************
* Multiply the quaternion by a vector (primarily used in determining axes)
**************************************************************************************************/
inline Vector3 Quaternion::operator*(const Vector3& rhs) const {
	Vector3 qVec(x, y, z);
	Vector3 uv, uuv;
	Vec3Cross(&uv, &qVec, &rhs);
	Vec3Cross(&uuv, &qVec, &uv);
	Vec3Scale(&uv, &uv, 2.0f * w);
	Vec3Scale(&uuv, &uuv, 2.0f);
	Vec3Add(&qVec, &rhs, &uv);
	Vec3Add(&qVec, &qVec, &uuv);
	return qVec;
}

/**************************************************************************************************
* Returns the dividend of the quaternion and a scalar
**************************************************************************************************/
inline Quaternion Quaternion::operator/(float rhs) const {
	const float f = 1.0f / rhs;
	return Quaternion(
		x*f,
		y*f,
		z*f,
		w*f);
}

/**************************************************************************************************
* Returns product of quaternion and a scalar
**************************************************************************************************/
inline Quaternion operator*(float lhs, const Quaternion& rhs) {
	return Quaternion(
		lhs*rhs.x,
		lhs*rhs.y,
		lhs*rhs.z,
		lhs*rhs.w);
}

/**************************************************************************************************
* Equivalence
**************************************************************************************************/
inline bool Quaternion::operator==(const Quaternion& rhs) const {
	return (x == rhs.x && y == rhs.y && z == rhs.z && w == rhs.w);
}

/**************************************************************************************************
* Non-equivalence
**************************************************************************************************/
inline bool Quaternion::operator!=(const Quaternion& rhs) const {
	return (x != rhs.x || y != rhs.y || z != rhs.z || w != rhs.w);
}

/**************************************************************************************************
* Add two quaternions together
**************************************************************************************************/
inline Quaternion* QuaternionAdd(Quaternion* pOut, const Quaternion* pQ1, const Quaternion* pQ2) {
	pOut->x = pQ1->x + pQ2->x;
	pOut->y = pQ1->y + pQ2->y;
	pOut->z = pQ1->z + pQ2->z;
	pOut->w = pQ1->w + pQ2->w;
	return pOut;
}

/**************************************************************************************************
* Difference between two quaternions
**************************************************************************************************/
inline Quaternion* QuaternionSub(Quaternion* pOut, const Quaternion* pQ1, const Quaternion* pQ2) {
	pOut->x = pQ1->x - pQ2->x;
	pOut->y = pQ1->y - pQ2->y;
	pOut->z = pQ1->z - pQ2->z;
	pOut->w = pQ1->w - pQ2->w;
	return pOut;
}

/**************************************************************************************************
* Scale the quaternion
**************************************************************************************************/
inline Quaternion* QuaternionScale(Quaternion* pOut, const Quaternion* pQ, float s) {
	pOut->x = pQ->x * s;
	pOut->y = pQ->y * s;
	pOut->z = pQ->z * s;
	pOut->w = pQ->w * s;
	return pOut;
}

/**************************************************************************************************
* Length
**************************************************************************************************/
inline float QuaternionLength(const Quaternion* pQ) {
	return sqrtf(pQ->x*pQ->x + pQ->y*pQ->y + pQ->z*pQ->z + pQ->w*pQ->w);
}

/**************************************************************************************************
* Length squared, or "norm"
**************************************************************************************************/
inline float QuaternionLengthSqr(const Quaternion* pQ) {
	return pQ->x*pQ->x + pQ->y*pQ->y + pQ->z*pQ->z + pQ->w*pQ->w;
}

/**************************************************************************************************
* Dot
**************************************************************************************************/
inline float QuaternionDot(const Quaternion* pQ1, const Quaternion* pQ2) {
	return pQ1->x*pQ2->x + pQ1->y*pQ2->y + pQ1->z*pQ2->z + pQ1->w*pQ2->w;
}

/**************************************************************************************************
* Identity (0, 0, 0, 1)
**************************************************************************************************/
inline Quaternion* QuaternionIdentity(Quaternion* pOut) {
	pOut->x = 0.0f;
	pOut->y = 0.0f;
	pOut->z = 0.0f;
	pOut->w = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Is identity
**************************************************************************************************/
inline bool QuaternionIsIdentity(const Quaternion* pQ) {
	return (pQ->x == 0.0f && pQ->y == 0.0f && pQ->z == 0.0f && pQ->w == 1.0f);
}

/**************************************************************************************************
* Conjugate (-x, -y, -z, -w)
**************************************************************************************************/
inline Quaternion* QuaternionConjugate(Quaternion* pOut, const Quaternion* pQ) {
	pOut->x = -pQ->x;
	pOut->y = -pQ->y;
	pOut->z = -pQ->z;
	pOut->w = -pQ->w;
	return pOut;
}